/*
                    Pathfinding Implementation
*/
#include "Pathfinding.h"

queue<int> Pathfinding::GeneratePath(Player *ptrPlayer, Point Start, Point End, int iDirection, vector<Player*>& AvoidList)
{
    queue<int> Return;

    if (Start.x == End.x && Start.y == End.y)
    {
        Return.push(iDirection);

        return Return;
    }

    for (int x = 0; x < NodeMap.size1(); x++)
    {
        for (int y = 0; y < NodeMap.size2(); y++)
        {
            NodeMap(x, y)->bOpen = false;
            NodeMap(x, y)->bClosed = false;
            NodeMap(x, y)->ptrParent = NULL;
        }
    }

    vector<Node*> OpenList;
    vector<Node*> ClosedList;

    OpenList.push_back(NodeMap((int)Start.x / TILE_WIDTH, (int)Start.y / TILE_HEIGHT));
    OpenList.back()->g = 0;
    OpenList.back()->h = GetNodeH(OpenList.back()->x, OpenList.back()->y, End);
    OpenList.back()->Calculate();

    bool bPathFound = false, bPathFinished = false;
    int iX = 0;
    int iY = 0;

    boost::numeric::ublas::matrix<int> m = CollisionMatrix;

    // Add the current snake's body to the collisions
 /*   for (int i = 0; i < ptrPlayer->getSnake().Body.size(); i++)
    {
        if (ptrPlayer->getSnake().Body.at(i)->Type != HEAD)
        {
            m((int)ptrPlayer->getSnake().Body.at(i)->Position.x / SNAKE_WIDTH, (int)ptrPlayer->getSnake().Body.at(i)->Position.y / SNAKE_HEIGHT) = 1;
        }
    }*/

    for (int i = 0; i < AvoidList.size(); i++)
    {
        if (AvoidList.at(i) != NULL)
        {
            // Add the current snake's body to the collisions
            for (int j = 0; j < AvoidList.at(i)->getSnake().Body.size(); j++)
            {
                m((int)AvoidList.at(i)->getSnake().Body.at(j)->Position.x / SNAKE_WIDTH, (int)AvoidList.at(i)->getSnake().Body.at(j)->Position.y / SNAKE_HEIGHT) = 1;
            }
        }
    }

    while (bPathFound == false)
    {
        // No path found so return the same direction they were already going
        if (OpenList.size() == 0)
        {
            Return.push(iDirection);

            bPathFound = true;

            break;
        }

        Node* currentNode = NodeMap(OpenList.back()->x, OpenList.back()->y);

        if (currentNode == NULL) // error that should never happen
        {
            Return.push(iDirection);

            bPathFound = true;

            break;
        }

        OpenList.pop_back();

        // Add the surrounding nodes to the open list ******************************************************************
        if (AddNodeToOpenList(m, OpenList, currentNode->x - 1, currentNode->y, currentNode, End) == true)
        {
            bPathFinished = true;
            iX = -1;
        }
        if (AddNodeToOpenList(m, OpenList, currentNode->x + 1, currentNode->y, currentNode, End) == true)
        {
            bPathFinished = true;
            iX = 1;
        }
        if (AddNodeToOpenList(m, OpenList, currentNode->x, currentNode->y - 1, currentNode, End) == true)
        {
            bPathFinished = true;
            iY = -1;
        }
        if (AddNodeToOpenList(m, OpenList, currentNode->x, currentNode->y + 1, currentNode, End) == true)
        {
            bPathFinished = true;
            iY = 1;
        }
        //**************************************************************************************************************

        AddNodeToClosedList(ClosedList, currentNode->x, currentNode->y);

        // Path found, generate the directions
        if (bPathFinished == true)
        {
            Node *temp = NodeMap(currentNode->x + iX, currentNode->y + iY);
            temp->ptrParent = currentNode;

            currentNode = temp;

            // While we're not back at the beginning
            while (currentNode->ptrParent != NULL)
            {
                if (currentNode->x == currentNode->ptrParent->x)
                {
                    if (currentNode->y < currentNode->ptrParent->y)
                    {
                        Return.push(DOWN);
                    }
                    else
                    {
                        Return.push(UP);
                    }
                }
                else if (currentNode->y == currentNode->ptrParent->y)
                {
                    if (currentNode->x < currentNode->ptrParent->x)
                    {
                        Return.push(RIGHT);
                    }
                    else
                    {
                        Return.push(LEFT);
                    }
                }
                else
                {
                    cout << "Path generation error: Invalid node location (" << currentNode->x << " x " << currentNode->y << ")" << endl;
                }

                currentNode = currentNode->ptrParent;
            }

            bPathFound = true;
        }
    }

    OpenList.clear();
    ClosedList.clear();

    return Return;
}

void Pathfinding::AddNodeToClosedList(vector<Node*>& ClosedList, int x, int y)
{
    Node* temp = NodeMap(x, y);
    temp->bOpen = false;
    temp->bClosed = true;

    ClosedList.push_back(temp);
}

bool Pathfinding::AddNodeToOpenList(boost::numeric::ublas::matrix<int>& m, vector<Node*>& OpenList, int x, int y, Node* Parent, const Point& End)
{
    if (x < 0 || x >= iWidth || y < 0 || y >= iHeight)
        return false;

    if (x == ((int)End.x / TILE_WIDTH) && y == ((int)End.y / TILE_HEIGHT))
        return true;

    if (m(x, y) != 0)
        return false;

    Node* temp = NodeMap(x, y);

    if (temp->bOpen == true || temp->bClosed == true)
        return false;

    temp->g = Parent->g + 1;
    temp->h = GetNodeH(temp->x, temp->y, End);
    temp->Calculate();
    temp->ptrParent = Parent;
    temp->bOpen = true;

    // Sorts the list from biggest to smallest
    for (int i = 0; i < OpenList.size(); i++)
    {
        if (OpenList.at(i)->f < temp->f)
        {
            OpenList.insert(OpenList.begin() + i, temp);

            return false;
        }
    }

    OpenList.push_back(temp);

    return false;
}

void Pathfinding::Initialise(Map& currentMap)
{
    iWidth = currentMap.getWidth() / TILE_WIDTH;
    iHeight = currentMap.getHeight() / TILE_HEIGHT;

    NodeMap.resize(iWidth, iHeight);

    Node *ptrTemp = NULL;

    FillMatrix(NodeMap, ptrTemp);

    CollisionMatrix = currentMap.getCollisionMatrix();

    for (int x = 0; x < NodeMap.size1(); x++)
    {
        for (int y = 0; y < NodeMap.size2(); y++)
        {
            NodeMap(x, y) = new Node(0, 0, x, y);
        }
    }
}

void Pathfinding::Quit()
{
    for (int x = 0; x < NodeMap.size1(); x++)
    {
        for (int y = 0; y < NodeMap.size2(); y++)
        {
            delete NodeMap(x, y);
        }
    }

    NodeMap.clear();
    CollisionMatrix.clear();
}

int Pathfinding::GetNodeH(int x, int y, const Point& End)
{
    int iX = (int)End.x - x;
    int iY = (int)End.y - y;

    if (x < 0) { x *= -1; }
    if (y < 0) { y *= -1; }

    return iX + iY;
}
